import rclpy
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener, Buffer
from tf_transformations import euler_from_quaternion

class TFListener(Node):
    def __init__(self):
        super().__init__("tf2_listener")
        self.buffer_ = Buffer()
        self.listner_ = TransformListener(self.buffer_,self)
        self.timer_ = self.create_timer(1,self.get_transfrom)

    def get_transfrom(self):
        try:
            result = self.buffer_.lookup_transform('map','base_footprint',
                    rclpy.time.Time(seconds=0),rclpy.time.Duration(seconds=1))
            transform = result.transform
            self.get_logger().info(f'平移:{transform.translation}')
            self.get_logger().info(f'旋转:{transform.rotation}')
            ratation_euler = euler_from_quaternion([
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w]
            )
            self.get_logger().info(f'平移rpy:{ratation_euler}')
        except Exception as e:
            self.get_logger().warn(f'获取坐标转换异常{str(e)}')

def main():
    rclpy.init()
    node = TFListener()
    rclpy.spin(node)
    rclpy.shutdown()